﻿using System;
using System.Diagnostics;

namespace NestedParticleFilter
{
    public class Point
    {  
        private double _x;   // in meters
        private double _z;   // in meters
        
        public Point()
        {
            _x = 0;
            _z = 0;
        }
        
        public Point(double x, double z)
        {
            _x = x;
            _z = z;

        }
        public double X
        {
            get { return _x; }
            set { _x = value; }
        }
        public double Z
        {
            get { return _z; }
            set { _z = value; }
        }
        
        public override String ToString()
        {
            return "(" + _x.ToString("##.##") + ", " + _z.ToString("##.##") + ")";
        }

    }//end of pose
    
    
    public class RobotPose :Point, ICloneable
    {
        double _theta; // in degree
        public double Theta
        {
            set { _theta = value; }
            get { return _theta; }
        }

        //Constructors
        public RobotPose()
        {
        }
        public RobotPose(double x, double z, double theta)
        {

            X = x;
            Z = z;
            Theta = theta; // in degree

        }// end of Constructor

        public override string ToString()
        {
            string str = "(x, z, theta) : ( " + X.ToString("#0.00") + ",  " + Z.ToString("#0.00") + ",  " + Theta.ToString("#0.00") + ")";
            return str;
        }

        public object Clone()
        {
            return  new RobotPose (X,Z,Theta);
        }


        #region Static Methods

        /// <summary>
        /// The angle (degree) in which the robot has to turn in order to face the object
        /// </summary>
        /// <param name="pose">Robot's pose</param>
        /// <param name="obj">object of interest</param>
        /// <returns></returns>
        /// <exception cref="ArgumentNullException"><paramref name="pose" /> is <c>null</c>.</exception>
        public static double RotateTowardObject(RobotPose pose, Point obj)
        {
            if (pose == null)
                throw  new ArgumentNullException("pose","pose cannot be null");
            if (obj == null)
                throw new ArgumentNullException("obj","obj cannot be null");
            double theta = Math.Atan2(-obj.X + pose.X, -obj.Z + pose.Z);     //θbetween the object and the robot in radian
            theta = UnitConversion.RadianToDegree(UnitConversion.PutThetaInBound(theta)); // in degree
            theta = pose.Theta - theta; // The amount of angle in which the robot has to turn (in degree)
            return theta;
        }
        #endregion 

        // end of ToString

    }// end of RobotPose class
}
